;-*2013*-*2013*-*2013*-*2013*-*2013*-*2013*-*2013*-*2013*-*2013*-
;
;HHS-SCIENCE-OLYMPIAD-2013-ROBOT-ARM-CONTROL-AHK-MAESTRO-CONTROL
;Version		: 1.0.1.1
;Authors		: Bryant Grey-Stewart (Mobius1), David "Pringles" Fellows (Bohr)
;Release Notes	: 1 - Clockwise - right - up // -1 - Counter-Clockwise - left - down

;Possition Variables: Storing the current possitions of the servos
varBaseTarg = 4500
varSholTarg = 4500
varElboTarg = 4500
varWrisTarg = 4500
varGripTarg = 4500

;Pin Variables: Which Slot are the parts in anyways
baseServo = 10
sholServo = 11
elboServo = 9
wrisServo = 8
gripServo = 7

;Lmit Variables: Define the minimum and maximum ranges of the servos
minBasePos = 700
minSholPos = 800
minElboPos = 600
minWrisPos = 600
minGripPos = 1100

maxBasePos = 9200
maxSholPos = 6800
maxElboPos = 9600
maxWrisPos = 9600
maxGripPos = 9600

;State Variables: Store the current 'state' of the robot
autoMode = 0
;hasInitialized = 0

;MAIN SEGMENT: PLACE HOTKEY CALLS, ROUTINES, ETC. HERE

!w::
MoveBase(1) ; Move right
return

!s::
MoveBase(-1) ; Move left
return

!a::
MoveShol(1) ; Move Up
return

!d::
MoveShol(-1) ; Move Down
return

!u::
MoveElbo(1) ; Move Up
return

!j::
MoveElbo(-1) ; Move Down
return

!h::
MoveWris(1) ; Move Up ? IDK
return

!k::
MoveWris(-1) ; Move Down ?
return

!r::
MoveGrip(1) ; Close
return

!f::
MoveGrip(-1) ; Open Who knows, we'll figure it out
return

;FUCTION SEGMENT: PLACE ANY FUCTIONS HERE
MoveBase(dir)
{
	if (dir > 0) ;If going forward
	{
		varBaseTarg = varBaseTarg + 250 ; add 250 millipulses to target
		;if(varBaseTarg < maxBasePos) ; if that doesn't break the max
		;{
			Run usccmd --servo %baseServo%,%sum%,,Hide ; move there
		;}
		;else
		;{
		varBaseTarg = varBaseTarg - 250 ; reset
		;}
	}
	else if (dir < 0)
	{
	;Run usccmd --servo 0`,7500,,Hide
		difference := servoBaseTarg - 250
		;if(difference > minBasePos) ; if that doesn't break the min
		;{
			Run usccmd --servo %servoBase%,%difference%,,Hide ; move there
			varBaseTarg := %difference% ; and save the new target
		;}
	}	
}

MoveShol(dir)
{
	if (dir > 0) ;If going forward
	{
		sum := servoSholTarg + 250 ; add 250 millipulses to target
		if(sum < maxSholPos) ; if that doesn't break the max
		{
			Run usccmd --servo %servoShol%,%sum%,,Hide ; move there
			varSholTarg := %sum% ; and save the new target
		}
	}
	else if (dir < 0)
	{
		difference := servoSholTarg - 250
		if(difference > minSholPos) ; if that doesn't break the min
		{
			Run usccmd --servo %servoShol%,%difference%,,Hide ; move there
			varSholTarg := %difference% ; and save the new target
		}
	}	
}

MoveElbo(dir)
{
	if (dir > 0) ;If going forward
	{
		sum := servoElboTarg + 250 ; add 250 millipulses to target
		if(sum < maxElboPos) ; if that doesn't break the max
		{
			Run usccmd --servo %servoElbo%,%sum%,,Hide ; move there
			varElboTarg := %sum% ; and save the new target
		}
	}
	else if (dir < 0)
	{
		difference := servoElboTarg - 250
		if(difference > minElboPos) ; if that doesn't break the min
		{
			Run usccmd --servo %servoElbo%,%difference%,,Hide ; move there
			varElboTarg := %difference% ; and save the new target
		}
	}	
}

MoveWris(dir)
{
	if (dir > 0) ;If going forward
	{
		sum := servoWrisTarg + 250 ; add 250 millipulses to target
		if(sum < maxWrisPos) ; if that doesn't break the max
		{
			Run usccmd --servo %servoWris%,%sum%,,Hide ; move there
			varWrisTarg := %sum% ; and save the new target
		}
	}
	else if (dir < 0)
	{
		difference := servoWrisTarg - 250
		if(difference > minWrisPos) ; if that doesn't break the min
		{
			Run usccmd --servo %servoWris%,%difference%,,Hide ; move there
			varWrisTarg := %difference% ; and save the new target
		}
	}	
}

MoveGrip(dir)
{
	if (dir > 0) ;If going forward
	{
		sum := servoGripTarg + 250
		if(sum < maxGripPos) ; if that doesn't break the max
		{
			Run usccmd --servo %servoGrip%,%sum%,,Hide ; move there
			varGripTarg := %sum% ; and save the new target
		}
	}
	else if (dir < 0)
	{
		difference := servoGripTarg - 250
		if(difference > minGripPos) ; if that doesn't break the min
		{
			Run usccmd --servo %servoGrip%,%difference%,,Hide ; move there
			varGripTarg := %difference% ; and save the new target
		}
	}	
}